{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {
    "colab_type": "text",
    "id": "EgiF12Hf1Dhs"
   },
   "source": [
    "This notebook provides examples to go along with the [textbook](http://manipulation.csail.mit.edu/robot.html).  I recommend having both windows open, side-by-side!"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "eeMrMI0-1Dhu"
   },
   "outputs": [],
   "source": [
    "from pydrake.all import (\n",
    "    DiagramBuilder,\n",
    "    MeshcatVisualizer,\n",
    "    Simulator,\n",
    "    StartMeshcat,\n",
    ")\n",
    "\n",
    "from manipulation.station import load_scenario, MakeHardwareStation\n",
    "from manipulation.utils import RenderDiagram"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Start the visualizer.\n",
    "meshcat = StartMeshcat()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "colab_type": "text",
    "id": "wJWL-ldv5REK"
   },
   "source": [
    "# A simple bimanual manipulation example\n",
    "\n",
    "The `MakeHardwareStation` code will add all of the controller code / logic for you, even if you have multiple iiwa/wsg-pairs in the scene."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "9GSF52A8ExQa"
   },
   "outputs": [],
   "source": [
    "scenario_data = \"\"\"\n",
    "directives:\n",
    "- add_model:\n",
    "    name: iiwa_left\n",
    "    file: package://drake/manipulation/models/iiwa_description/iiwa7/iiwa7_no_collision.sdf\n",
    "    default_joint_positions:\n",
    "        iiwa_joint_1: [-1.57]\n",
    "        iiwa_joint_2: [0.1]\n",
    "        iiwa_joint_3: [0]\n",
    "        iiwa_joint_4: [-1.2]\n",
    "        iiwa_joint_5: [0]\n",
    "        iiwa_joint_6: [ 1.6]\n",
    "        iiwa_joint_7: [0]\n",
    "- add_weld:\n",
    "    parent: world\n",
    "    child: iiwa_left::iiwa_link_0\n",
    "- add_model:\n",
    "    name: wsg_left\n",
    "    file: package://drake/manipulation/models/wsg_50_description/sdf/schunk_wsg_50_with_tip.sdf\n",
    "- add_weld:\n",
    "    parent: iiwa_left::iiwa_link_7\n",
    "    child: wsg_left::body\n",
    "    X_PC:\n",
    "        translation: [0, 0, 0.09]\n",
    "        rotation: !Rpy { deg: [90, 0, 90]}\n",
    "- add_model:\n",
    "    name: iiwa_right\n",
    "    file: package://drake/manipulation/models/iiwa_description/iiwa7/iiwa7_no_collision.sdf\n",
    "    default_joint_positions:\n",
    "        iiwa_joint_1: [-1.57]\n",
    "        iiwa_joint_2: [0.1]\n",
    "        iiwa_joint_3: [0]\n",
    "        iiwa_joint_4: [-1.2]\n",
    "        iiwa_joint_5: [0]\n",
    "        iiwa_joint_6: [ 1.6]\n",
    "        iiwa_joint_7: [0]\n",
    "- add_weld:\n",
    "    parent: world\n",
    "    child: iiwa_right::iiwa_link_0\n",
    "    X_PC:\n",
    "        translation: [.6, 0, 0]\n",
    "        rotation: !Rpy { deg: [0, 0, 0]}\n",
    "- add_model:\n",
    "    name: wsg_right\n",
    "    file: package://drake/manipulation/models/wsg_50_description/sdf/schunk_wsg_50_with_tip.sdf\n",
    "- add_weld:\n",
    "    parent: iiwa_right::iiwa_link_7\n",
    "    child: wsg_right::body\n",
    "    X_PC:\n",
    "        translation: [0, 0, 0.09]\n",
    "        rotation: !Rpy { deg: [90, 0, 90]}\n",
    "model_drivers:\n",
    "    iiwa_left: !IiwaDriver\n",
    "      hand_model_name: wsg_left\n",
    "    wsg_left: !SchunkWsgDriver {}\n",
    "    iiwa_right: !IiwaDriver\n",
    "      hand_model_name: wsg_right\n",
    "    wsg_right: !SchunkWsgDriver {}\n",
    "\"\"\"\n",
    "\n",
    "scenario = load_scenario(data=scenario_data)\n",
    "builder = DiagramBuilder()\n",
    "\n",
    "station = builder.AddSystem(MakeHardwareStation(scenario))\n",
    "plant = station.GetSubsystemByName(\"plant\")\n",
    "controller_plant = station.GetSubsystemByName(\n",
    "    \"iiwa_left.controller\"\n",
    ").get_multibody_plant_for_control()\n",
    "\n",
    "visualizer = MeshcatVisualizer.AddToBuilder(\n",
    "    builder, station.GetOutputPort(\"query_object\"), meshcat\n",
    ")\n",
    "\n",
    "diagram = builder.Build()\n",
    "\n",
    "RenderDiagram(diagram, max_depth=1)\n",
    "\n",
    "simulator = Simulator(diagram)\n",
    "context = simulator.get_mutable_context()\n",
    "station_context = station.GetMyMutableContextFromRoot(context)\n",
    "plant_context = plant.GetMyMutableContextFromRoot(context)\n",
    "\n",
    "q0 = plant.GetPositions(\n",
    "    plant_context, plant.GetModelInstanceByName(\"iiwa_left\")\n",
    ")\n",
    "station.GetInputPort(\"iiwa_left.position\").FixValue(station_context, q0)\n",
    "station.GetInputPort(\"iiwa_right.position\").FixValue(station_context, q0)\n",
    "\n",
    "station.GetInputPort(\"wsg_left.position\").FixValue(station_context, [0])\n",
    "station.GetInputPort(\"wsg_right.position\").FixValue(station_context, [0])\n",
    "\n",
    "# Confirm that simulation works:\n",
    "simulator.AdvanceTo(0.1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "5SjOClhTltPk"
   },
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "colab": {
   "collapsed_sections": [],
   "name": "Robotic Manipulation - Let's get you a robot.ipynb",
   "provenance": [],
   "toc_visible": true
  },
  "kernelspec": {
   "display_name": "Python 3.11.0 64-bit",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.10.12"
  },
  "vscode": {
   "interpreter": {
    "hash": "5c7b89af1651d0b8571dde13640ecdccf7d5a6204171d6ab33e7c296e100e08a"
   }
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
